#ifdef VS2008
#include "../stdafx.h"
#else
#include "stdafx.h"
#endif

#include <cmath>

#include "fbl.h"
#include "ETF.h"

extern void MakeGaussianVector(double sigma, myvec& GAU);

#define ABS(x) ( ((x)>0) ? (x) : (-(x)) )
#define round(x) ((int) ((x) + 0.5))
#define dist3(x, y, z) sqrt(((double)x)*((double)x) + ((double)y)*((double)y) + ((double)z)*((double)z))

inline double gauss1D_bl(double x, double mean, double sigma)
{
	return ( exp( (-x*x) / (2*sigma*sigma) ) );
}

inline void MakeGaussVectorBL(double sigma, myvec& GAU, int max_index)
{
	int i;
	//////////////////////////////////////////////
	GAU.init(max_index); // size of GAU vector
	GAU.zero(); 

	GAU[0] = gauss1D_bl((double)0.0, 0.0, sigma);
	for (i = 1; i < GAU.getMax(); i++) {
		GAU[i] = gauss1D_bl((double)i, 0.0, sigma);
	}
}



void GetFBL(cimatrix& image, ETF& e, double sigma, double sigma2, double sigma3, double sigma4, int max_itr)
// flow-based color bilateral filter
// sigma: spatial parameter in tangent direction
// sigma2: spatial parameter in gradient direction
// sigma3: color parameter in tangent direction
// sigma4: color parameter in gradient direction
// max_itr: number of iterations
{
	int k, m;
	char r, g, b;
	int i, j, i_x, i_y;

	myvec vn(2), vt(2), w(2);
	double x, y, d_x, d_y;
	int s;
	int x1, y1;
	int d1;

	double sum_r, sum_g, sum_b;
	double weight, w_sum;
	double c_val_r, c_val_g, c_val_b;
	double val_r, val_g, val_b;
	double c_dist;

	int image_x = image.getRow();
	int image_y = image.getCol();

	int half_l, half_w;

	cimatrix tmp;
	tmp.copy(image);

	myvec GAU1, GAU2, GAU3, GAU4;
	MakeGaussianVector(sigma, GAU1); // length of the kernel
	MakeGaussianVector(sigma2, GAU2); // width of the kernel

	half_l = GAU1.getMax()-1;
	half_w = GAU2.getMax()-1;

	MakeGaussVectorBL(sigma3, GAU3, 512); // 300 array elements created
	MakeGaussVectorBL(sigma4, GAU4, 512); // 300 array elements created
	
	double step_size1;

	step_size1 = 1.0;

	for (m = 0; m < max_itr; m++) {
		
		/////////////////////////////////////////
		// FBL along tangent axis!
		for (j = 0; j < image_y; j++) {
			for (i = 0; i < image_x; i++) {
				//sum = 0.0;
				sum_r = sum_g = sum_b = 0.0;
				weight = 0.0;
				w_sum = 0.0;
				////////////////////////////////////////////////
				// One half
				d_x = (double)i; d_y = (double)j; 
				i_x = i;	i_y = j;
				////////////////////////////////////////////////
				// Use the center color at each minor axis!
				c_val_r = (double)tmp[i_x][i_y].r;
				c_val_g = (double)tmp[i_x][i_y].g;
				c_val_b = (double)tmp[i_x][i_y].b;
				//////////////////////
				for (k = 0; k < half_l; k++) {
					vt[0] = e[i_x][i_y].tx;
					vt[1] = e[i_x][i_y].ty;
					if (vt[0] == 0.0 && vt[1] == 0.0) break;
					////////////////////////////////////////////////
					x = d_x;
					y = d_y;
					/////////////////////////////////////////////////////
					if (x > (double)image_x-1 || x < 0.0 || y > (double)image_y-1 || y < 0.0) 
						continue;
					/////////////////////////////////////////////////
					x1 = round(x);	if (x1 < 0) x1 = 0; if (x1 > image_x-1) x1 = image_x-1;
					y1 = round(y);	if (y1 < 0) y1 = 0; if (y1 > image_y-1) y1 = image_y-1;
					val_r = (double)tmp[x1][y1].r;
					val_g = (double)tmp[x1][y1].g;
					val_b = (double)tmp[x1][y1].b;
					/////////////////////////////////////////////////////////
					weight = GAU1[k]; 
					c_dist = dist3(c_val_r-val_r, c_val_g-val_g, c_val_b-val_b);
					weight *= GAU3[ (int)(c_dist/1.732) ]; // like Gaussian!
					/////////////////////////////////////////////////////
					sum_r += val_r * weight;
					sum_g += val_g * weight;
					sum_b += val_b * weight;
					w_sum += weight;
					/////////////////////////////////////////
					d_x += vt[0] * step_size1; 
					d_y += vt[1] * step_size1; 
					/////////////////////////////////////////
					i_x = round(d_x); 
					i_y = round(d_y); 
					if (d_x < 0 || d_x > image_x-1 || d_y < 0 || d_y > image_y-1) break;
				}
				////////////////////////////////////////////////
				// Other half
				d_x = (double)i; d_y = (double)j; 
				i_x = i;	i_y = j;
				for (k = 0; k < half_l; k++) {
					vt[0] = -e[i_x][i_y].tx;
					vt[1] = -e[i_x][i_y].ty;
					if (vt[0] == 0.0 && vt[1] == 0.0) break;
					///////////////////////////////
					x = d_x;
					y = d_y;
					/////////////////////////////////////////////////////
					if (x > (double)image_x-1 || x < 0.0 || y > (double)image_y-1 || y < 0.0) 
						continue;
					/////////////////////////////////////////////////
					x1 = round(x);	if (x1 < 0) x1 = 0; if (x1 > image_x-1) x1 = image_x-1;
					y1 = round(y);	if (y1 < 0) y1 = 0; if (y1 > image_y-1) y1 = image_y-1;
					val_r = (double)tmp[x1][y1].r;
					val_g = (double)tmp[x1][y1].g;
					val_b = (double)tmp[x1][y1].b;
					/////////////////////////////////////////////////////////
					weight = GAU1[k];
					c_dist = dist3(c_val_r-val_r, c_val_g-val_g, c_val_b-val_b);
					weight *= GAU3[ (int)(c_dist/1.732) ]; // like Gaussian!
					/////////////////////////////////////////////////////
					sum_r += val_r * weight;
					sum_g += val_g * weight;
					sum_b += val_b * weight;
					w_sum += weight;
					/////////////////////////////////////////
					d_x += vt[0] * step_size1; // accurate new location x
					d_y += vt[1] * step_size1; // accurate new location y
					/////////////////////////////////////////
					i_x = round(d_x); // integer version of new location x
					i_y = round(d_y); // integer version of new location y
					if (d_x < 0 || d_x > image_x-1 || d_y < 0 || d_y > image_y-1) break;
				}
				sum_r /= (double)w_sum; // normalize!
				sum_g /= (double)w_sum; // normalize!
				sum_b /= (double)w_sum; // normalize!
				/////////////////////////////////////
				if (e[i][j].tx == 0.0 && e[i][j].ty == 0.0) {
					sum_r = tmp[i][j].r;
					sum_g = tmp[i][j].g;
					sum_b = tmp[i][j].b;
				}
				/////////////////////////////////////
				r = (char)sum_r;
				g = (char)sum_g;
				b = (char)sum_b;
				image[i][j].r = r;
				image[i][j].g = g;
				image[i][j].b = b;
			}
		}
		tmp.copy(image); // update tmp

		/////////////////////////////////////////
		// FBL along gradient axis!
		for (j = 0; j < image_y; j++) {
			for (i = 0; i < image_x; i++) {
				sum_r = sum_g = sum_b = 0.0;
				weight = 0.0;
				w_sum = 0.0;
				////////////////////////////////////////////////
				d_x = (double)i; d_y = (double)j; 
				i_x = i;	i_y = j;
				vn[0] = -e[i_x][i_y].ty;
				vn[1] = e[i_x][i_y].tx;
				if (vn[0] == 0.0 && vn[1] == 0.0) break;
				////////////////////////////////////////////////
				c_val_r = (double)tmp[i_x][i_y].r;
				c_val_g = (double)tmp[i_x][i_y].g;
				c_val_b = (double)tmp[i_x][i_y].b;
				////////////////////////////////////////////////
				for (s = -half_w; s <= half_w; s++) { // width of Gaussian kernel
					////////////////////////
					x = d_x + vn[0] * s;
					y = d_y + vn[1] * s;
					/////////////////////////////////////////////////////
					if (x > (double)image_x-1 || x < 0.0 || y > (double)image_y-1 || y < 0.0) 
						continue;
					/////////////////////////////////////////////////
					x1 = round(x);	if (x1 < 0) x1 = 0; if (x1 > image_x-1) x1 = image_x-1;
					y1 = round(y);	if (y1 < 0) y1 = 0; if (y1 > image_y-1) y1 = image_y-1;
					val_r = (double)tmp[x1][y1].r;
					val_g = (double)tmp[x1][y1].g;
					val_b = (double)tmp[x1][y1].b;
					/////////////////////////////////////////////////////////
					d1 = ABS(s);
					weight = GAU2[d1];
					c_dist = dist3(c_val_r-val_r, c_val_g-val_g, c_val_b-val_b);
					weight *= GAU4[ (int)(c_dist/1.732) ]; // like Gaussian!
					/////////////////////////////////////////////////////
					sum_r += val_r * weight;
					sum_g += val_g * weight;
					sum_b += val_b * weight;
					w_sum += weight;
				}
				/////////////////////////////////////////
				sum_r /= (double)w_sum; // normalize!
				sum_g /= (double)w_sum; // normalize!
				sum_b /= (double)w_sum; // normalize!
				/////////////////////////////////////
				if (e[i][j].tx == 0.0 && e[i][j].ty == 0.0) {
					sum_r = tmp[i][j].r;
					sum_g = tmp[i][j].g;
					sum_b = tmp[i][j].b;
				}
				/////////////////////////////////////
				r = (char)sum_r;
				g = (char)sum_g;
				b = (char)sum_b;
				image[i][j].r = r;
				image[i][j].g = g;
				image[i][j].b = b;
			}
		}
		tmp.copy(image); // update tmp
	}
}

void ConstructMergedImageColor(cimatrix& image, imatrix& gray, cimatrix& merged) 
{
	int x, y;

	int image_x = image.getRow();
	int image_y = image.getCol();

	for (y = 0; y < image_y; y++) {
		for (x = 0; x < image_x; x++) {
			if (gray[x][y] == 0) {
				merged[x][y].r = 0;
				merged[x][y].g = 0;
				merged[x][y].b = 0;
			}
			else {
				merged[x][y].r = image[x][y].r;
				merged[x][y].g = image[x][y].g;
				merged[x][y].b = image[x][y].b;
			}
		}
	}
}

